/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-07-15     MBW       the first version
 */
#include "JY901B_931.h"
#include "maths.h"
#include "filter.h"
#include "string.h"
#include "stdlib.h"
#include "stdio.h"
#include "drv_common.h"
#include "uart_config.h"
#include "board.h"
#include <rtdevice.h>
#include <rtthread.h>
//#include "task_prio.h"
struct STime        stcTime;
struct SAcc         stcAcc;
struct SGyro        stcGyro;
struct SAngle   stcAngle;
struct SMag         stcMag;
struct SDStatus stcDStatus;
struct SPress   stcPress;
struct SLonLat  stcLonLat;
struct SGPSV        stcGPSV;
struct SQ       stcQ;

JY_Sensor JY901_data={0};
_Matrix Mat = {0};
task_time time_all;
#define SAMPLE_UART_NAME       "uart2"      /* 串口设备名称 */
/* 用于接收消息的信号量 */
static rt_device_t serial;
u8 get_u6data=0 , u6_tran =0;
u8 rev_buffer[512];
u16 u6_count=0;
static struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;  /* 初始化配置参数 */
biquadFilter_t accFilterLPF[3]; //加速度数据二阶低通滤波器
biquadFilter_t gyroFilterLPF[3];//陀螺仪数据二阶低通滤波器
void init_JY931_901B(void)//13
{

      /* 查找串口设备 */
      serial = rt_device_find(SAMPLE_UART_NAME);
      if (!serial)
      {
          rt_kprintf("find %s failed!\n", SAMPLE_UART_NAME);
      }
      else rt_kprintf("find %s is ok!\n", SAMPLE_UART_NAME);
      /* step2：修改串口配置参数 */
      config.baud_rate = BAUD_RATE_115200;        //修改波特率为
//      config.baud_rate = BAUD_RATE_9600;        //修改波特率为
      config.data_bits = DATA_BITS_8;           //数据位 8
      config.stop_bits = STOP_BITS_1;           //停止位 1
      config.bufsz     = 128;                   //修改缓冲区 buff size 为 128
      config.parity    = PARITY_NONE;           //无奇偶校验位
      /* step3：控制串口设备。通过控制接口传入命令控制字，与控制参数 */
      rt_device_control(serial, RT_DEVICE_CTRL_CONFIG, &config);
      /* step4：打开串口设备。以中断接收及轮询发送模式打开串口设备 */
      rt_device_open(serial, RT_DEVICE_FLAG_INT_RX);
      /* 以读写及中断接收方式打开串口设备 */
      rt_device_open(serial, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX);
      /* 设置接收回调函数 */
      rt_device_set_rx_indicate(serial, uart_input);
     /* 发送字符串 */
  //    rt_device_write(serial, 0, str, (sizeof(str) - 1));
      rt_thread_mdelay(10);
      rt_kprintf("init_JY931_901B  is ok \n");
      for (int axis = 0; axis < 3; axis++)//con_fre Hz
      {
          biquadFilterInitLPF(&gyroFilterLPF[axis], 200, 80);
          biquadFilterInitLPF(&accFilterLPF[axis],  200, 15.0);
      }

      rt_kprintf("init_JY931_901B biquadFilterInitLPF is ok \n");

      JY901_data.show = 0;
}
INIT_APP_EXPORT(init_JY931_901B);
void get_JY901B(void)
{
    float fil_acc[3],fil_gyro[3];
    float q0,q1,q2,q3;

    JY901_data.angel.y = (float)stcAngle.Angle[0]/32768*180;//roll
    JY901_data.angel.x  = (float)stcAngle.Angle[1]/32768*180;//pitch
    JY901_data.angel.z  = (float)stcAngle.Angle[2]/32768*180;//yaw
    fil_gyro[0] = (float)stcGyro.w[0]/32768*2000;
    fil_gyro[1] = (float)stcGyro.w[1]/32768*2000;
    fil_gyro[2] = (float)stcGyro.w[2]/32768*2000;
    fil_acc[0]  = (float)stcAcc.a[0]/32768*16;
    fil_acc[1]  = (float)stcAcc.a[1]/32768*16;
    fil_acc[2]  = (float)stcAcc.a[2]/32768*16;
    JY901_data.quaternion.q0 = (float)stcQ.q[0]/32768;
    JY901_data.quaternion.q1 = (float)stcQ.q[1]/32768;
    JY901_data.quaternion.q2 = (float)stcQ.q[2]/32768;
    JY901_data.quaternion.q3 = (float)stcQ.q[3]/32768;
    JY901_data.height=(float)stcPress.lAltitude/100;
    JY901_data.press =stcPress.lPressure;
    JY901_data.gpsHeight = (float)stcGPSV.sGPSHeight/10;
    JY901_data.GPSYaw = (float)stcGPSV.sGPSYaw/10;
    JY901_data.GPSV = (float)stcGPSV.lGPSVelocity/1000;
    JY901_data.Longitude = (float)stcLonLat.lLon/1000 ;
    JY901_data.Lattitude = (float)stcLonLat.lLat/1000 ;
    JY901_data.ucYear = stcTime.ucYear;  JY901_data.ucMonth = stcTime.ucMonth; JY901_data.ucDay = stcTime.ucDay;
    JY901_data.ucHour = stcTime.ucHour;  JY901_data.ucMinute = stcTime.ucMinute; JY901_data.ucSecond = stcTime.ucSecond;

    JY901_data.gyro_ori.x = fil_gyro[0] ; JY901_data.gyro_ori.y = fil_gyro[1] ; JY901_data.gyro_ori.z = fil_gyro[2] ;

    q0 = (float)stcQ.q[0]/32768;
    q1 = (float)stcQ.q[1]/32768;
    q2 = (float)stcQ.q[2]/32768;
    q3 = (float)stcQ.q[3]/32768;
    Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
    Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);
    Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);

    Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);
    Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
    Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);

    Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);
    Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);
    Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
//    JY901_data.angel.x = asin(2*(q0*q2 - q1*q3 )) * RAD2DEG ; // 俯仰   换算成度
//    JY901_data.angel.y = asin(2*(q0*q1 + q2*q3 )) * RAD2DEG ; // 横滚

    for (int axis = 0; axis < 3; axis++)
    {
        fil_acc[axis]   = biquadFilterApply(&accFilterLPF[axis], fil_acc[axis]);
        fil_gyro[axis]  = biquadFilterApply(&gyroFilterLPF[axis],fil_gyro[axis]);
    }
    JY901_data.gyro.x  = fil_gyro[0];
    JY901_data.gyro.y  = fil_gyro[1];
    JY901_data.gyro.z  = fil_gyro[2];
    JY901_data.aac.x   = fil_acc[0];
    JY901_data.aac.y   = fil_acc[1];
    JY901_data.aac.z   = fil_acc[2];

}
/* 接收数据回调函数 */
rt_err_t uart_input(rt_device_t dev, rt_size_t size)
{
    static unsigned char ucRxBuffer[250] ;
    static unsigned char ucRxCnt = 0  ,sc=0 ;
    unsigned char ucData;

    rt_device_read(serial, -1, &ucData, 1);
    ucRxBuffer[ucRxCnt++]=ucData;


    if (ucRxBuffer[0]!=0x55) //Êý¾ÝÍ·²»¶Ô£¬ÔòÖØÐÂ¿ªÊ¼Ñ°ÕÒ0x55Êý¾ÝÍ·
    {
        ucRxCnt=0;
        return 0;
    }
    if (ucRxCnt<11) {return;}//Êý¾Ý²»Âú11¸ö£¬Ôò·µ»Ø
    else
    {
        for (int var = 0; var < 11; ++var)
        {
            sc+=ucRxBuffer[var];
        }
        if(sc&ucRxBuffer[10])
        {
//            get_u6data = 1;
//            if(get_u6data==1 && u6_tran==1)
//            {
                for (int var = 0; var < 11; ++var)
                {
                    rev_buffer[var] = ucRxBuffer[var];
                }
//            }

            switch(ucRxBuffer[1])//ÅÐ¶ÏÊý¾ÝÊÇÄÄÖÖÊý¾Ý£¬È»ºó½«Æä¿½±´µ½¶ÔÓ¦µÄ½á¹¹ÌåÖÐ£¬ÓÐÐ©Êý¾Ý°üÐèÒªÍ¨¹ýÉÏÎ»»ú´ò¿ª¶ÔÓ¦µÄÊä³öºó£¬²ÅÄÜ½ÓÊÕµ½Õâ¸öÊý¾Ý°üµÄÊý¾Ý
            {
                case 0x50:  memcpy(&stcTime,&ucRxBuffer[2],8);break;//memcpyÎª±àÒëÆ÷×Ô´øµÄÄÚ´æ¿½±´º¯Êý£¬ÐèÒýÓÃ"string.h"£¬½«½ÓÊÕ»º³åÇøµÄ×Ö·û¿½±´µ½Êý¾Ý½á¹¹ÌåÀïÃæ£¬´Ó¶øÊµÏÖÊý¾ÝµÄ½âÎö¡£
                case 0x51:  memcpy(&stcAcc,&ucRxBuffer[2],8);break;
                case 0x52:  memcpy(&stcGyro,&ucRxBuffer[2],8);break;
                case 0x53:  memcpy(&stcAngle,&ucRxBuffer[2],8);break;
                case 0x54:  memcpy(&stcMag,&ucRxBuffer[2],8);break;
                case 0x55:  memcpy(&stcDStatus,&ucRxBuffer[2],8);break;
                case 0x56:  memcpy(&stcPress,&ucRxBuffer[2],8);break;
                case 0x57:  memcpy(&stcLonLat,&ucRxBuffer[2],8);break;
                case 0x58:  memcpy(&stcGPSV,&ucRxBuffer[2],8);break;
                case 0x59:  memcpy(&stcQ,&ucRxBuffer[2],8);break;
            }
        }
        sc=0;
        get_u6data=0,u6_tran=0;
        ucRxCnt=0;//Çå¿Õ»º´æÇø
    }
}

